#include "rads_base.hpp"
#include <ros/ros.h>
#include "rads/rads_scmd.h"
#include "rads/rads_trackext.h"
#include "rads/rads_chart.h"
#include <sstream> // 用于字符串流
#include <vector>  // 用于整数数组
#include <thread>
using namespace std;
ros::Publisher p;
// rtk&&雷达所有数据
typedef struct
{
  int point_count;
  double x;
  double y;
  double z;
  double ox;
  double oy;
  double oz;
  double ow;
  double yaw;
  double matching_rate;
  double longitude;
  double latitude;
  double altitude;
  double heading;
  double groud_speed;
  double posi_flag;
  double head_flag;
  double ext;
  double flag;
  double rtk_x;
  double rtk_y;
  double rtk_z;
  double f_x;
  double f_y;
  double f_z;
  double ld_heading;
  double base_length;
} locate_information;
locate_information lidar_rtk;
typedef struct
{
    int model;
    int soc;
    int err;
    int rpm_l;
    int rpm_r;
    int key_1;
    int key_2;
    int key_3;
    int key_4;
    int key_5;
    int key_6;
    int key_7;
    int key_8;
    int ladar_1;
    int ladar_2;
    int ladar_3;
    int ladar_4;
    int ladar_5;
    int ladar_6;
    int ladar_7;
    int ladar_8;
    int unknow_1;
    int unknow_2;
    int unknow_3;
    int unknow_4;
    int unknow_5;
    int unknow_6;
} carpcb;
carpcb car_pcd;
// 接受融合后的雷达+RTK数据
void laserCallback(const rads::rads_trackext::ConstPtr &msg)
{
    // 获取雷达数据
    lidar_rtk.x = isnan(msg->x) ? 0.0 : msg->x;
    lidar_rtk.y = isnan(msg->y) ? 0.0 : msg->y;
    lidar_rtk.z = isnan(msg->z) ? 0.0 : msg->z;
    lidar_rtk.yaw = isnan(msg->heading) ? 0.0 : msg->heading;
    lidar_rtk.matching_rate = isnan(msg->matching_rate) ? 0.0 : msg->matching_rate;
    // 获取RTK数据
    lidar_rtk.rtk_x = isnan(msg->rtk_x) ? 0.0 : msg->rtk_x;
    lidar_rtk.rtk_y = isnan(msg->rtk_y) ? 0.0 : msg->rtk_y;
    lidar_rtk.rtk_z = isnan(msg->rtk_z) ? 0.0 : msg->rtk_z;
    lidar_rtk.heading = isnan(msg->heading) ? 0.0 : msg->heading;
    lidar_rtk.longitude = isnan(msg->longitude) ? 0.0 : msg->longitude;
    lidar_rtk.latitude = isnan(msg->latitude) ? 0.0 : msg->latitude;
    lidar_rtk.posi_flag = isnan(msg->posi_flag) ? 0.0 : msg->posi_flag;
    lidar_rtk.head_flag = isnan(msg->head_flag) ? 0.0 : msg->head_flag;
    lidar_rtk.groud_speed = isnan(msg->groud_speed) ? 0.0 : msg->groud_speed;
    lidar_rtk.f_x=0.0;
    lidar_rtk.f_y=0.0;
    lidar_rtk.f_z=0.0;

}

// 下位机来的数据
void pradsscmdCallback(const rads::rads_scmd::ConstPtr &msg)
{
    car_pcd.model =isnan(msg->model) ? 0 : msg->model; // （1:遥控，2:跟随和推，3:返航，4:无人驾驶）
    car_pcd.soc = isnan(msg->soc) ? 0 : msg->soc;
}

void publishThread() {
    ros::Rate rate(10); // 10Hz发布频率
    
    rads::rads_chart msg;
    
    // 初始化消息内容（示例值）
    msg.header.frame_id = "map";
    
    while (ros::ok()) {
        // 更新时间戳
        msg.header.stamp = ros::Time::now();
        
        // 填充消息数据（示例值，实际应用中应替换为真实数据源）
        msg.x = 1.0;
        msg.y = 2.0;
        msg.z = 3.0;
        msg.yaw = 0.785; // 约45度
        msg.matching_rate = 0.95;
        msg.longitude = 119.2222;
        msg.latitude = 22.333;
        msg.altitude = 4.0;
        msg.heading = 5.0;
        msg.groud_speed = 6.0;
        msg.posi_flag = 7.0;
        msg.head_flag = 8.0;
        msg.rtk_x = 9.0;
        msg.rtk_y = 10.0;
        msg.rtk_z = 11.0;
        msg.f_x = 12.0;
        msg.f_y = 13.0;
        msg.f_z = 14.0;
        msg.model = 1;
        msg.soc = 30;
                
        // 发布消息
        p.publish(msg);
        
        rate.sleep();
    }
}


int main(int argc, char** argv) {
    ros::init(argc, argv, NODE_CHART);
    ros::NodeHandle nh;
    // 订阅下位机发来的指令 pradsscmd
    ros::Subscriber sub_pradsscmd = nh.subscribe("pradsscmd", 1, pradsscmdCallback);
     // 订阅融合后的雷达+RTK数据
    ros::Subscriber ndt_pose = nh.subscribe("pradsfusion", 1, laserCallback); 
    //发布任务完成后话题
    p = nh.advertise<rads::rads_chart>("/pradschart", QUEUE_SIZE);
    std::thread pub_thread(publishThread);

    ros::spin();
    if (pub_thread.joinable()) {
        pub_thread.join();
    }
    return 0;
}